#pragma once
#include "reconstruct.cuh"
using namespace cv;
#define Vec3f SLCuda::Vec3f
namespace SLCuda
{
    __device__ void getEpipolarLine(float *point, float *fundamentalMatrix, float *epipolarLine)
    {
        float a = fundamentalMatrix[0] * point[0] + fundamentalMatrix[1] * point[1] + fundamentalMatrix[2] * 1.0f;
        float b = fundamentalMatrix[3] * point[0] + fundamentalMatrix[4] * point[1] + fundamentalMatrix[5] * 1.0f;
        float c = fundamentalMatrix[6] * point[0] + fundamentalMatrix[7] * point[1] + fundamentalMatrix[8] * 1.0f;
        float nu = a * a + b * b;
        nu = nu ? 1. / sqrt(nu) : 1.;
        a *= nu;
        b *= nu;
        c *= nu;
        epipolarLine[0] = a;
        epipolarLine[1] = b;
        epipolarLine[2] = c;
    }

    __device__ void lineIntersect(float *line1, float *line2, float *res)
    {
        float x[3];
        cross(line1, line2, x);
        res[0] = x[0] / x[2];
        res[1] = x[1] / x[2];
    }

    __device__ void undistortPoint(float *p, float *instrinsic, float *kc, float *res)
    {
        int iters = 5;
        float x = p[1];
        float y = p[0];
        float fx = instrinsic[0];
        float fy = instrinsic[4];
        float ifx = 1. / fx;
        float ify = 1. / fy;
        float cx = instrinsic[2];
        float cy = instrinsic[5];

        float u = x;
        float v = y;
        float x0 = (x - cx) * ifx;
        float y0 = (y - cy) * ify;
        x = x0;
        y = y0;

        float k[8] = {0};
        for (int i = 0; i < 5; i++)
        {
            k[i] = kc[i];
        }

        for (int j = 0; j < iters; j++)
        {
            double r2 = x * x + y * y;
            double icdist = (1 + ((k[7] * r2 + k[6]) * r2 + k[5]) * r2) / (1 + ((k[4] * r2 + k[1]) * r2 + k[0]) * r2);
            double deltaX = 2 * k[2] * x * y + k[3] * (r2 + 2 * x * x);
            double deltaY = k[2] * (r2 + 2 * y * y) + 2 * k[3] * x * y;
            x = (x0 - deltaX) * icdist;
            y = (y0 - deltaY) * icdist;
        }

        double xx = instrinsic[0] * x + instrinsic[1] * y + instrinsic[2];
        double yy = instrinsic[3] * x + instrinsic[4] * y + instrinsic[5];
        double ww = 1. / (instrinsic[6] * x + instrinsic[7] * y + instrinsic[8]);
        res[1] = xx * ww;
        res[0] = yy * ww;
    }

    __device__ float length(float *p)
    {
        return sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
    }

    __device__ float *normalize(float *p)
    {
        float length = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
        float r[3];
        r[0] = p[0] / length;
        r[1] = p[1] / length;
        r[2] = p[2] / length;
        return r;
    }

    __device__ float dot(float *p1, float *p2)
    {
        return p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2];
    }

    __device__ float *cross(float *p1, float *p2)
    {
        float *r = new float[3];

        r[0] = p1[1] * p2[2] - p1[2] * p2[1];
        r[1] = p1[2] * p2[0] - p1[0] * p2[2];
        r[2] = p1[0] * p2[1] - p1[1] * p2[0];

        return r;
    }

    __device__ void cross(float *p1, float *p2, float *r)
    {
        r[0] = p1[1] * p2[2] - p1[2] * p2[1];
        r[1] = p1[2] * p2[0] - p1[0] * p2[2];
        r[2] = p1[0] * p2[1] - p1[1] * p2[0];
    }

    __device__ float getAngleCos(float *p1, float *p2)
    {
        float *normalizedP1 = normalize(p1);
        float *normalizedP2 = normalize(p2);
        return dot(normalizedP1, normalizedP2);
    }

    __device__ float getAngleCos(Vec3f p1, Vec3f p2)
    {
        Vec3f normalizedP1 = p1.normalize();
        Vec3f normalizedP2 = p2.normalize();
        return normalizedP1.dot(normalizedP2);
    }

    __device__ float getAngle(float *p1, float *p2)
    {
        float *normalizedP1 = normalize(p1);
        float *normalizedP2 = normalize(p2);
        return acos(dot(normalizedP1, normalizedP2));
    }

    __device__ float getAngle(Vec3f p1, Vec3f p2)
    {
        Vec3f normalizedP1 = p1.normalize();
        Vec3f normalizedP2 = p2.normalize();
        return acos(normalizedP1.dot(normalizedP2));
    }

    __device__ void multiply_44_41(float *mat, float *p, float *r)
    {
        r[0] = mat[0] * p[0] + mat[1] * p[1] + mat[2] * p[2] + mat[3] * p[3];
        r[1] = mat[4] * p[0] + mat[5] * p[1] + mat[6] * p[2] + mat[7] * p[3];
        r[2] = mat[8] * p[0] + mat[9] * p[1] + mat[10] * p[2] + mat[11] * p[3];
        r[3] = mat[12] * p[0] + mat[13] * p[1] + mat[14] * p[2] + mat[15] * p[3];
    }

    __device__ void multiply_33_31(float *mat, float *p, float *r)
    {
        r[0] = mat[0] * p[0] + mat[1] * p[1] + mat[2] * p[2];
        r[1] = mat[3] * p[0] + mat[4] * p[1] + mat[5] * p[2];
        r[2] = mat[6] * p[0] + mat[7] * p[1] + mat[8] * p[2];
    }

    __device__ Vec3f Vec3f::operator+(const Vec3f &other) const
    {
        return Vec3f(x + other.x, y + other.y, z + other.z);
    }

    __device__ Vec3f Vec3f::operator-(const Vec3f &other) const
    {
        return Vec3f(x - other.x, y - other.y, z - other.z);
    }

    __device__ Vec3f Vec3f::operator*(const float s) const
    {
        return Vec3f(x * s, y * s, z * s);
    }

    __device__ Vec3f Vec3f::operator/(const float s) const
    {
        return Vec3f(x / s, y / s, z / s);
    }

    __device__ Vec3f Vec3f::normalize()
    {
        float length = sqrt(x * x + y * y + z * z);
        return Vec3f(x / length, y / length, z / length);
    }

    __device__ void Vec3f::normalizeSelf()
    {
        float length = sqrt(x * x + y * y + z * z);
        x /= length;
        y /= length;
        z /= length;
    }

    __device__ float Vec3f::length()
    {
        float length = sqrt(x * x + y * y + z * z);
        return length;
    }

    __device__ float Vec3f::dot(const Vec3f &other)
    {
        return x * other.x + y * other.y + z * other.z;
    }

    __device__ void Vec3f::cross(const Vec3f &other, Vec3f &r)
    {
        r.x = y * other.z - z * other.y;
        r.y = z * other.x - x * other.z;
        r.z = x * other.y - y * other.x;
    }

    __device__ Vec3f Vec3f::cross(const Vec3f &other)
    {
        Vec3f r;
        r.x = y * other.z - z * other.y;
        r.y = z * other.x - x * other.z;
        r.z = x * other.y - y * other.x;
        return r;
    }

    __device__ void triangulatePoint(float *cameraPoint, float *projectorPoint, float *cameraIntrinsic, float *projectorIntrinsic, float *extrinsicMat, float *extrinsicRotation, float *point)
    {
        float cameraIntrinsicTx = cameraIntrinsic[2];
        float cameraIntrinsicTy = cameraIntrinsic[5];
        float cameraIntrinsicFx = cameraIntrinsic[0];
        float cameraIntrinsicFy = cameraIntrinsic[4];
        float projectorIntrinsicTx = projectorIntrinsic[2];
        float projectorIntrinsicTy = projectorIntrinsic[5];
        float projectorIntrinsicFx = projectorIntrinsic[0];
        float projectorIntrinsicFy = projectorIntrinsic[4];

        Vec3f projectorPoint3d = {0, 0, 0};

        float projectorDirectionX = (projectorPoint[0] - projectorIntrinsicTx) / projectorIntrinsicFx;
        float projectorDirectionY = (projectorPoint[1] - projectorIntrinsicTy) / projectorIntrinsicFy;
        Vec3f projectorDirection = {projectorDirectionX, projectorDirectionY, 1};
        projectorDirection = projectorDirection.normalize();

        float cameraPoint4d[] = {0, 0, 0, 1};
        float cameraPoint4dMat[4];
        multiply_44_41(extrinsicMat, cameraPoint4d, cameraPoint4dMat);
        Vec3f cameraPoint3d = {cameraPoint4dMat[0], cameraPoint4dMat[1], cameraPoint4dMat[2]};

        float cameraDirectionX = (cameraPoint[0] - cameraIntrinsicTx) / cameraIntrinsicFx;
        float cameraDirectionY = (cameraPoint[1] - cameraIntrinsicTy) / cameraIntrinsicFy;
        float cameraDirection3d[] = {cameraDirectionX, cameraDirectionY, 1};
        float *normalizedCameraDirection3d = normalize(cameraDirection3d);
        float cameraDirection3dMat[3];
        multiply_33_31(extrinsicRotation, normalizedCameraDirection3d, cameraDirection3dMat);
        Vec3f cameraDirection = {cameraDirection3dMat[0], cameraDirection3dMat[1], cameraDirection3dMat[2]};
        cameraDirection = cameraDirection.normalize();

        Vec3f normal = cameraDirection.cross(projectorDirection).normalize();
        float originDistance = (projectorPoint3d - cameraPoint3d).length();
        Vec3f originVec = cameraPoint3d - projectorPoint3d;
        Vec3f originVecNormalized = originVec / originDistance;
        Vec3f projectorPointInCameraPlane = cameraPoint3d - originVec - normal * originDistance * abs(getAngleCos(originVec, normal));
        Vec3f cameraPointInProjectorPlane = projectorPoint3d + originVec + normal * originDistance * abs(getAngleCos(originVec, normal));

        float theta1 = getAngle(projectorPointInCameraPlane - cameraPoint3d, cameraDirection);
        float theta2 = PI - getAngle(projectorPointInCameraPlane - cameraPoint3d, projectorDirection);
        float theta3 = PI - theta1 - theta2;
        float x = (projectorPointInCameraPlane - cameraPoint3d).length() / sin(theta3) * sin(theta2);
        Vec3f p1 = cameraPoint3d + cameraDirection * x;

        theta1 = getAngle(projectorPoint3d - cameraPointInProjectorPlane, cameraDirection);
        theta2 = PI - getAngle(projectorPoint3d - cameraPointInProjectorPlane, projectorDirection);
        theta3 = PI - theta1 - theta2;
        x = (cameraPointInProjectorPlane - projectorPoint3d).length() / sin(theta3) * sin(theta2);
        Vec3f p2 = cameraPointInProjectorPlane + cameraDirection * x;

        point[0] = (p1.x + p2.x) / 2;
        point[1] = (p1.y + p2.y) / 2;
        point[2] = (p1.z + p2.z) / 2;
    }

    __global__ void
    reconstructPhaseShiftMonocularCudaKernel(uchar *image, float *decodeImage, Size imageSize, float *fundamental, float *cameraIntrinsic, float *cameraKc, float *projectorIntrinsic, float *projectorKc, float *extrinsicMat, float *extrinsicRotation, float minDistance, float maxDistance, float *reconstutionPoint, uchar *reconstrutionColor)
    {
        int tid = blockIdx.x * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
        int h = tid / imageSize.width;
        int w = tid % imageSize.width;
        float x = decodeImage[tid];
        if (isnan(x))
        {
            reconstutionPoint[tid * 3] = NAN;
            reconstutionPoint[tid * 3 + 1] = NAN;
            reconstutionPoint[tid * 3 + 2] = NAN;
            reconstrutionColor[tid] = NAN;
            return;
        }
        float xLine[3]{1, 0, -x};
        float cameraPoint[2]{w, h};
        float epipolarLine[3];
        getEpipolarLine(cameraPoint, fundamental, epipolarLine);
        float projectorPoint[2];
        lineIntersect(xLine, epipolarLine, projectorPoint);
        float cameraPointUndistorted[3];
        undistortPoint(cameraPoint, cameraIntrinsic, cameraKc, cameraPointUndistorted);
        float projectorPointUndistorted[3];
        undistortPoint(projectorPoint, projectorIntrinsic, projectorKc, projectorPointUndistorted);

        float point3d[3];
        triangulatePoint(cameraPointUndistorted, projectorPointUndistorted, cameraIntrinsic, projectorIntrinsic, extrinsicMat, extrinsicRotation, point3d);
        if (point3d[2] < maxDistance && point3d[2] > minDistance)
        {

            reconstutionPoint[tid * 3] = point3d[0];
            reconstutionPoint[tid * 3 + 1] = point3d[1];
            reconstutionPoint[tid * 3 + 2] = point3d[2];
            reconstrutionColor[tid] = image[tid];
        }
        else
        {
            reconstutionPoint[tid * 3] = NAN;
            reconstutionPoint[tid * 3 + 1] = NAN;
            reconstutionPoint[tid * 3 + 2] = NAN;
            reconstrutionColor[tid] = NAN;
        }
    }

    void reconstructPhaseShiftMonocularCuda(Mat &image, Mat &decodeImage, Size imageSize, Size patternSize, SL::ProjectorCalibrateTrinsics &projectorCalibrateTrinics, float minDistance, float maxDistance, SL::Points &points, SL::Colors &colors)
    {
        int length = imageSize.height * imageSize.width;
        int memSizeUchar = imageSize.height * imageSize.width * sizeof(uchar);
        int memSizeFloat = imageSize.height * imageSize.width * sizeof(float);
        int memSizeInt = imageSize.height * imageSize.width * sizeof(int);

        float *decodeImageDevice;
        cudaMalloc((void **)&decodeImageDevice, memSizeFloat);
        cudaMemcpy(decodeImageDevice, decodeImage.data, memSizeFloat, cudaMemcpyHostToDevice);

        uchar *imageDevice;
        cudaMalloc((void **)&imageDevice, memSizeUchar);
        cudaMemcpy(imageDevice, image.data, memSizeUchar, cudaMemcpyHostToDevice);

        Mat cameraProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
        Mat projectorProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
        SL::Intrinsic &cameraIntrinsic = projectorCalibrateTrinics.cameraIntrinsic;
        SL::Intrinsic &projectorIntrinsic = projectorCalibrateTrinics.projectorIntrinsic;
        Mat &fundamental = projectorCalibrateTrinics.extrinsic.fundamental;

        float *fundamentalDevice;
        cudaMalloc((void **)&fundamentalDevice, 9 * sizeof(float));
        cudaMemcpy(fundamentalDevice, fundamental.data, 9 * sizeof(float), cudaMemcpyHostToDevice);

        float *cameraIntrinsicDevice;
        cudaMalloc((void **)&cameraIntrinsicDevice, 9 * sizeof(float));
        cudaMemcpy(cameraIntrinsicDevice, cameraIntrinsic.matrix.data, 9 * sizeof(float), cudaMemcpyHostToDevice);
        float *cameraKcDevice;
        cudaMalloc((void **)&cameraKcDevice, 5 * sizeof(float));
        cudaMemcpy(cameraKcDevice, cameraIntrinsic.kc.data(), 5 * sizeof(float), cudaMemcpyHostToDevice);

        float *projectorIntrinsicDevice;
        cudaMalloc((void **)&projectorIntrinsicDevice, 9 * sizeof(float));
        cudaMemcpy(projectorIntrinsicDevice, projectorIntrinsic.matrix.data, 9 * sizeof(float), cudaMemcpyHostToDevice);
        float *projectorKcDevice;
        cudaMalloc((void **)&projectorKcDevice, 5 * sizeof(float));
        cudaMemcpy(projectorKcDevice, projectorIntrinsic.kc.data(), 5 * sizeof(float), cudaMemcpyHostToDevice);

        float *extrinsicMatDevice;
        cudaMalloc((void **)&extrinsicMatDevice, 16 * sizeof(float));
        cudaMemcpy(extrinsicMatDevice, projectorCalibrateTrinics.extrinsic.matrix.data, 16 * sizeof(float), cudaMemcpyHostToDevice);

        float *extrinsicRotationDevice;
        cudaMalloc((void **)&extrinsicRotationDevice, 9 * sizeof(float));
        cudaMemcpy(extrinsicRotationDevice, projectorCalibrateTrinics.extrinsic.rotation.data, 9 * sizeof(float), cudaMemcpyHostToDevice);

        float *reconstutionPoint;
        uchar *reconstrutionColor;
        cudaMalloc((void **)&reconstutionPoint, 3 * memSizeFloat);
        cudaMalloc((void **)&reconstrutionColor, memSizeUchar);

        dim3 block(1 + (length / (16 * 16 + 1)), 1, 1);
        dim3 thread(16, 16, 1);
        reconstructPhaseShiftMonocularCudaKernel<<<block, thread>>>(imageDevice, decodeImageDevice, imageSize, fundamentalDevice, cameraIntrinsicDevice, cameraKcDevice, projectorIntrinsicDevice, projectorKcDevice, extrinsicMatDevice, extrinsicRotationDevice, minDistance, maxDistance, reconstutionPoint, reconstrutionColor);

        float *pointsHost = new float[length * 3];
        cudaMemcpy(pointsHost, reconstutionPoint, 3 * memSizeFloat, cudaMemcpyDeviceToHost);
        for (int i = 0; i < length; i++)
        {
            if (!isnan(pointsHost[3 * i]))
            {
                int h = i / imageSize.width;
                int w = i % imageSize.width;
                Point3f p(pointsHost[3 * i], pointsHost[3 * i + 1], pointsHost[3 * i + 2]);
                points.push_back(p);
                uchar color = image.at<uchar>(h, w);
                // Point3f colorPoint(color, color, color);
                SL::Color colorPoint = {color, color, color};
                colors.push_back(colorPoint);
            }
        }
        // std::cout << points.size() << std::endl;
    }
}
// void reconstructPhaseShiftMonocularCuda(Mat &image, Mat &decodeImage, Size imageSize, Size patternSize, SL::ProjectorCalibrateTrinsics &projectorCalibrateTrinics, SL::Points &points, SL::Points &colors)
// {
//     int length = imageSize.height * imageSize.width;
//     int memSizeUchar = imageSize.height * imageSize.width * sizeof(uchar);
//     int memSizeFloat = imageSize.height * imageSize.width * sizeof(float);
//     int memSizeInt = imageSize.height * imageSize.width * sizeof(int);

//     float *decodeImageDevice;
//     cudaMalloc((void **)&decodeImageDevice, memSizeFloat);
//     cudaMemcpy(decodeImageDevice, decodeImage.data, memSizeFloat, cudaMemcpyHostToDevice);

//     uchar *imageDevice;
//     cudaMalloc((void **)&imageDevice, memSizeUchar);
//     cudaMemcpy(imageDevice, image.data, memSizeUchar, cudaMemcpyHostToDevice);

// Mat cameraProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
// Mat projectorProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
// SL::Intrinsic &cameraIntrinsic = projectorCalibrateTrinics.cameraIntrinsic;
// SL::Intrinsic &projectorIntrinsic = projectorCalibrateTrinics.projectorIntrinsic;
// cameraIntrinsic.matrix(Range(0, 3), Range(0, 3)).copyTo(cameraProjectionMatrix(Range(0, 3), Range(0, 3)));
// projectorIntrinsic.matrix(Range(0, 3), Range(0, 3)).copyTo(projectorProjectionMatrix(Range(0, 3), Range(0, 3)));
// projectorProjectionMatrix = projectorProjectionMatrix * projectorCalibrateTrinics.extrinsic.matrix;
// Mat &fundamental = projectorCalibrateTrinics.extrinsic.fundamental;

// float *fundamentalDevice;
// cudaMalloc((void **)&fundamentalDevice, 9 * sizeof(float));
// cudaMemcpy(fundamentalDevice, fundamental.data, 9 * sizeof(float), cudaMemcpyHostToDevice);

// float *cameraIntrinsicDevice;
// cudaMalloc((void **)&cameraIntrinsicDevice, 9 * sizeof(float));
// cudaMemcpy(cameraIntrinsicDevice, cameraIntrinsic.matrix.data, 9 * sizeof(float), cudaMemcpyHostToDevice);
// float *cameraKcDevice;
// cudaMalloc((void **)&cameraKcDevice, 5 * sizeof(float));
// cudaMemcpy(cameraKcDevice, cameraIntrinsic.kc.data(), 5 * sizeof(float), cudaMemcpyHostToDevice);

// float *projectorIntrinsicDevice;
// cudaMalloc((void **)&projectorIntrinsicDevice, 9 * sizeof(float));
// cudaMemcpy(projectorIntrinsicDevice, projectorIntrinsic.matrix.data, 9 * sizeof(float), cudaMemcpyHostToDevice);
// float *projectorKcDevice;
// cudaMalloc((void **)&projectorKcDevice, 5 * sizeof(float));
// cudaMemcpy(projectorKcDevice, projectorIntrinsic.kc.data(), 5 * sizeof(float), cudaMemcpyHostToDevice);

// float *cameraProjectionMatrixDevice;
// cudaMalloc((void **)&cameraProjectionMatrixDevice, 12 * sizeof(float));
// cudaMemcpy(cameraProjectionMatrixDevice, cameraProjectionMatrix.data, 12 * sizeof(float), cudaMemcpyHostToDevice);

// float *projectorProjectionMatrixDevice;
// cudaMalloc((void **)&projectorProjectionMatrixDevice, 12 * sizeof(float));
// cudaMemcpy(projectorProjectionMatrixDevice, projectorProjectionMatrix.data, 12 * sizeof(float), cudaMemcpyHostToDevice);

// float *reconstutionPoint;
// uchar *reconstrutionColor;
// int *isValidDevice;
// cudaMalloc((void **)&reconstutionPoint, 3 * memSizeFloat);
// cudaMalloc((void **)&reconstrutionColor, memSizeUchar);
// cudaMalloc((void **)&isValidDevice, memSizeInt);

// dim3 block(1 + (length / (16 * 16 + 1)), 1, 1);
// dim3 thread(16, 16, 1);

// // SVD reigon
// int m = 6;
// int n = 4;
// int batchSize = length;

// float *ADevice;
// cudaMalloc((void **)&ADevice, m * n * batchSize * sizeof(float));
// float *UDevice;
// cudaMalloc((void **)&UDevice, m * m * batchSize * sizeof(float));
// float *SDevice;
// cudaMalloc((void **)&SDevice, n * n * batchSize * sizeof(float));
// float *VDevice;
// cudaMalloc((void **)&VDevice, n * n * batchSize * sizeof(float));

// cusolverDnHandle_t cusolverH;
// cusolverDnCreate(&cusolverH);

// gesvdjInfo_t params;
// cusolverDnCreateGesvdjInfo(&params);
// cusolverDnXgesvdjSetTolerance(params, 1e-2);
// cusolverDnXgesvdjSetMaxSweeps(params, 10);
// cusolverDnXgesvdjSetSortEig(params, true);

// int workSize;
// auto statusBuffer = cusolverDnSgesvdjBatched_bufferSize(
//     cusolverH,
//     CUSOLVER_EIG_MODE_VECTOR,
//     m,
//     n,
//     ADevice,
//     m,
//     SDevice,
//     UDevice,
//     m,
//     VDevice,
//     n,
//     &workSize,
//     params,
//     batchSize);
// float *workDevice;
// int *info;
// // if (CUSOLVER_STATUS_SUCCESS == statusBuffer)
// // {
// cudaMalloc((void **)&workDevice, workSize);
// cudaMalloc((void **)&info, batchSize);

// reconstructPhaseShiftMonocularCudaKernelStage1<<<block, thread>>>(imageDevice, decodeImageDevice, imageSize, fundamentalDevice, cameraIntrinsicDevice, cameraKcDevice, projectorIntrinsicDevice, projectorKcDevice, cameraProjectionMatrixDevice, projectorProjectionMatrixDevice, ADevice, isValidDevice);

// statusBuffer = cusolverDnSgesvdjBatched(
//     cusolverH,
//     CUSOLVER_EIG_MODE_VECTOR,
//     m, n,
//     ADevice,
//     m,
//     SDevice,
//     UDevice,
//     m,
//     VDevice,
//     n,
//     workDevice,
//     workSize,
//     info,
//     params,
//     batchSize);
// // if (CUSOLVER_STATUS_SUCCESS == statusBuffer)
// // {
// reconstructPhaseShiftMonocularCudaKernelStage2<<<block, thread>>>(decodeImageDevice, VDevice, n, reconstutionPoint);
// // cudaThreadSynchronize();
// //     }
// // }

// float *pointsHost = new float[length * 3];
// cudaMemcpy(pointsHost, reconstutionPoint, 3 * memSizeFloat, cudaMemcpyDeviceToHost);
// for (int i = 0; i < length; i++)
// {
//     if (!isnan(pointsHost[3 * i]))
//     {
//         int h = i / imageSize.width;
//         int w = i - h * imageSize.width;
//         Point3f p(pointsHost[3 * i], pointsHost[3 * i + 1], pointsHost[3 * i + 2]);
//         points.push_back(p);
//         float color = (float)image.at<uchar>(h, w);
//         Point3f colorPoint = {color, color, color};
//         colors.push_back(colorPoint);
//     }
// }
// cudaFree(ADevice);
// cudaFree(SDevice);
// cudaFree(UDevice);
// cudaFree(VDevice);
// cudaFree(workDevice);
// std::cout << "cuda finish" << std::endl;
// }

// __device__ Vec3f triangulatePoint(Vec2f cameraPoint, Vec2f projectorPoint, float *cameraProjectionMatrix, float *projectorProjectionMatrix)
// {
//     Eigen::MatrixXd A(6, 4);
//     for (int k = 0; k < 4; k++)
//     {
//         A(0, k) = cameraPoint[0] * cameraProjectionMatrix[8 + k] - cameraProjectionMatrix[k];
//         A(1, k) = cameraPoint[1] * cameraProjectionMatrix[8 + k] - cameraProjectionMatrix[4 + k];
//         A(2, k) = cameraPoint[0] * cameraProjectionMatrix[4 + k] - cameraProjectionMatrix[k];
//         A(0, k) = projectorPoint[0] * projectorProjectionMatrix[8 + k] - projectorProjectionMatrix[k];
//         A(1, k) = projectorPoint[1] * projectorProjectionMatrix[8 + k] - projectorProjectionMatrix[4 + k];
//         A(2, k) = projectorPoint[0] * projectorProjectionMatrix[4 + k] - projectorProjectionMatrix[k];
//     }
//     Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
//     Eigen::VectorXd singularValues = svd.singularValues();
//     Eigen::MatrixXd U = svd.matrixU();
//     Eigen::MatrixXd V = svd.matrixV();
//     float x = V(0, 0) / V(3, 0);
//     float y = V(0, 0) / V(3, 0);
//     float z = V(0, 0) / V(3, 0);
//     return Vec3f{x, y, z};
// }

// __global__ void reconstructPhaseShiftMonocularCudaKernelStage1(uchar *image, float *decodeImage, Size imageSize, float *fundamental, float *cameraIntrinsic, float *cameraKc, float *projectorIntrinsic, float *projectorKc, float *cameraProjectionMatrix, float *projectorProjectionMatrix, float *ADevice, int *isValidDevice)
// {
//     int tid = blockIdx.x * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
//     int h = tid / imageSize.width;
//     int w = tid - h * imageSize.width;
//     float x = decodeImage[tid];
//     if (isnan(x))
//     {
//         isValidDevice[tid] = -1;
//         return;
//         // reconstutionPoint[tid * 3] = NAN;
//         // reconstutionPoint[tid * 3 + 1] = NAN;
//         // reconstutionPoint[tid * 3 + 2] = NAN;
//         // for (int k = 0; k < 4; k++)
//         // {
//         //     ADevice[24 * tid + 6 * k] = 0;
//         //     ADevice[24 * tid + 6 * k + 1] = 0;
//         //     ADevice[24 * tid + 6 * k + 2] = 0;
//         //     ADevice[24 * tid + 6 * k + 3] = 0;
//         //     ADevice[24 * tid + 6 * k + 4] = 0;
//         //     ADevice[24 * tid + 6 * k + 5] = 0;
//         // }
//         // return;
//     }
//     isValidDevice[tid] = 1;
//     float xLine[3]{1, 0, -x};
//     float cameraPoint[2]{w, h};
//     float epipolarLine[3];
//     getEpipolarLine(cameraPoint, fundamental, epipolarLine);
//     float projectorPoint[2];
//     lineIntersect(xLine, epipolarLine, projectorPoint);
//     float cameraPointUndistorted[3];
//     undistortPoint(cameraPoint, cameraIntrinsic, cameraKc, cameraPointUndistorted);
//     float projectorPointUndistorted[3];
//     undistortPoint(projectorPoint, cameraIntrinsic, cameraKc, projectorPointUndistorted);

//     for (int k = 0; k < 4; k++)
//     {
//         ADevice[24 * tid + 6 * k] = cameraPoint[0] * cameraProjectionMatrix[8 + k] - cameraProjectionMatrix[k];
//         ADevice[24 * tid + 6 * k + 1] = cameraPoint[1] * cameraProjectionMatrix[8 + k] - cameraProjectionMatrix[4 + k];
//         ADevice[24 * tid + 6 * k + 2] = cameraPoint[0] * cameraProjectionMatrix[4 + k] - cameraPoint[1] * cameraProjectionMatrix[k];
//         ADevice[24 * tid + 6 * k + 3] = projectorPoint[0] * projectorProjectionMatrix[8 + k] - projectorProjectionMatrix[k];
//         ADevice[24 * tid + 6 * k + 4] = projectorPoint[1] * projectorProjectionMatrix[8 + k] - projectorProjectionMatrix[4 + k];
//         ADevice[24 * tid + 6 * k + 5] = projectorPoint[0] * projectorProjectionMatrix[4 + k] - projectorPoint[1] * projectorProjectionMatrix[k];
//     }
//     // Vec3f reconstructPoint = triangulatePoint(cameraPoint, projectorPoint, cameraProjectionMatrix, projectorProjectionMatrix);
//     // reconstutionPoint[tid * 3] = reconstructPoint[0];
//     // reconstutionPoint[tid * 3 + 1] = reconstructPoint[1];
//     // reconstutionPoint[tid * 3 + 2] = reconstructPoint[2];
//     // reconstrutionColor[tid] = image[tid];
// }

// __global__ void reconstructPhaseShiftMonocularCudaKernelStage2(float *decodeImage, float *VDevice, int n, float *reconstutionPoint)
// {
//     int tid = blockIdx.x * blockDim.x * blockDim.y + threadIdx.y * blockDim.x + threadIdx.x;
//     if (isnan(decodeImage[tid]))
//     {
//         reconstutionPoint[tid * 3] = NAN;
//         reconstutionPoint[tid * 3 + 1] = NAN;
//         reconstutionPoint[tid * 3 + 2] = NAN;
//         return;
//     }
//     reconstutionPoint[tid * 3] = VDevice[n * n * tid] / VDevice[n * n * tid + 3];
//     reconstutionPoint[tid * 3 + 1] = VDevice[n * n * tid + 1] / VDevice[n * n * tid + 3];
//     reconstutionPoint[tid * 3 + 2] = VDevice[n * n * tid + 2] / VDevice[n * n * tid + 3];
// }
// void reconstructPhaseShiftMonocularCuda(Mat &image, Mat &decodeImage, Size imageSize, Size patternSize, SL::ProjectorCalibrateTrinsics &projectorCalibrateTrinics, SL::Points &points, SL::Points &colors)
// {
//     // int length = imageSize.height * imageSize.width;
//     // int memSizeUchar = imageSize.height * imageSize.width * sizeof(uchar);
//     // int memSizeFloat = imageSize.height * imageSize.width * sizeof(float);
//     // int memSizeInt = imageSize.height * imageSize.width * sizeof(int);

//     // float *decodeImageDevice;
//     // cudaMalloc((void **)&decodeImageDevice, memSizeFloat);
//     // cudaMemcpy(decodeImageDevice, decodeImage.data, memSizeFloat, cudaMemcpyHostToDevice);

//     // uchar *imageDevice;
//     // cudaMalloc((void **)&imageDevice, memSizeUchar);
//     // cudaMemcpy(imageDevice, image.data, memSizeUchar, cudaMemcpyHostToDevice);

//     // Mat cameraProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
//     // Mat projectorProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
//     // SL::Intrinsic &cameraIntrinsic = projectorCalibrateTrinics.cameraIntrinsic;
//     // SL::Intrinsic &projectorIntrinsic = projectorCalibrateTrinics.projectorIntrinsic;
//     // cameraIntrinsic.matrix(Range(0, 3), Range(0, 3)).copyTo(cameraProjectionMatrix(Range(0, 3), Range(0, 3)));
//     // projectorIntrinsic.matrix(Range(0, 3), Range(0, 3)).copyTo(projectorProjectionMatrix(Range(0, 3), Range(0, 3)));
//     // projectorProjectionMatrix = projectorProjectionMatrix * projectorCalibrateTrinics.extrinsic.matrix;
//     // Mat &fundamental = projectorCalibrateTrinics.extrinsic.fundamental;

//     // float *fundamentalDevice;
//     // cudaMalloc((void **)&fundamentalDevice, 9 * sizeof(float));
//     // cudaMemcpy(fundamentalDevice, fundamental.data, 9 * sizeof(float), cudaMemcpyHostToDevice);

//     // float *cameraIntrinsicDevice;
//     // cudaMalloc((void **)&cameraIntrinsicDevice, 9 * sizeof(float));
//     // cudaMemcpy(cameraIntrinsicDevice, cameraIntrinsic.matrix.data, 9 * sizeof(float), cudaMemcpyHostToDevice);
//     // float *cameraKcDevice;
//     // cudaMalloc((void **)&cameraKcDevice, 5 * sizeof(float));
//     // cudaMemcpy(cameraKcDevice, cameraIntrinsic.kc.data(), 5 * sizeof(float), cudaMemcpyHostToDevice);

//     // float *projectorIntrinsicDevice;
//     // cudaMalloc((void **)&projectorIntrinsicDevice, 9 * sizeof(float));
//     // cudaMemcpy(projectorIntrinsicDevice, projectorIntrinsic.matrix.data, 9 * sizeof(float), cudaMemcpyHostToDevice);
//     // float *projectorKcDevice;
//     // cudaMalloc((void **)&projectorKcDevice, 5 * sizeof(float));
//     // cudaMemcpy(projectorKcDevice, projectorIntrinsic.kc.data(), 5 * sizeof(float), cudaMemcpyHostToDevice);

//     // float *cameraProjectionMatrixDevice;
//     // cudaMalloc((void **)&cameraProjectionMatrixDevice, 12 * sizeof(float));
//     // cudaMemcpy(cameraProjectionMatrixDevice, cameraProjectionMatrix.data, 12 * sizeof(float), cudaMemcpyHostToDevice);

//     // float *projectorProjectionMatrixDevice;
//     // cudaMalloc((void **)&projectorProjectionMatrixDevice, 12 * sizeof(float));
//     // cudaMemcpy(projectorProjectionMatrixDevice, projectorProjectionMatrix.data, 12 * sizeof(float), cudaMemcpyHostToDevice);

//     // float *reconstutionPoint;
//     // uchar *reconstrutionColor;
//     // int *isValidDevice;
//     // cudaMalloc((void **)&reconstutionPoint, 3 * memSizeFloat);
//     // cudaMalloc((void **)&reconstrutionColor, memSizeUchar);
//     // cudaMalloc((void **)&isValidDevice, memSizeInt);

//     // dim3 block(1 + (length / (16 * 16 + 1)), 1, 1);
//     // dim3 thread(16, 16, 1);

//     // // SVD reigon
//     // int m = 6;
//     // int n = 4;
//     // int batchSize = length;

//     // float *ADevice;
//     // cudaMalloc((void **)&ADevice, m * n * batchSize * sizeof(float));
//     // float *UDevice;
//     // cudaMalloc((void **)&UDevice, m * m * batchSize * sizeof(float));
//     // float *SDevice;
//     // cudaMalloc((void **)&SDevice, n * n * batchSize * sizeof(float));
//     // float *VDevice;
//     // cudaMalloc((void **)&VDevice, n * n * batchSize * sizeof(float));

//     // cusolverDnHandle_t cusolverH;
//     // cusolverDnCreate(&cusolverH);

//     // gesvdjInfo_t params;
//     // cusolverDnCreateGesvdjInfo(&params);
//     // cusolverDnXgesvdjSetTolerance(params, 1e-2);
//     // cusolverDnXgesvdjSetMaxSweeps(params, 10);
//     // cusolverDnXgesvdjSetSortEig(params, true);

//     // int workSize;
//     // auto statusBuffer = cusolverDnSgesvdjBatched_bufferSize(
//     //     cusolverH,
//     //     CUSOLVER_EIG_MODE_VECTOR,
//     //     m,
//     //     n,
//     //     ADevice,
//     //     m,
//     //     SDevice,
//     //     UDevice,
//     //     m,
//     //     VDevice,
//     //     n,
//     //     &workSize,
//     //     params,
//     //     batchSize);
//     // float *workDevice;
//     // int *info;
//     // // if (CUSOLVER_STATUS_SUCCESS == statusBuffer)
//     // // {
//     // cudaMalloc((void **)&workDevice, workSize);
//     // cudaMalloc((void **)&info, batchSize);

//     // reconstructPhaseShiftMonocularCudaKernelStage1<<<block, thread>>>(imageDevice, decodeImageDevice, imageSize, fundamentalDevice, cameraIntrinsicDevice, cameraKcDevice, projectorIntrinsicDevice, projectorKcDevice, cameraProjectionMatrixDevice, projectorProjectionMatrixDevice, ADevice, isValidDevice);

//     // statusBuffer = cusolverDnSgesvdjBatched(
//     //     cusolverH,
//     //     CUSOLVER_EIG_MODE_VECTOR,
//     //     m, n,
//     //     ADevice,
//     //     m,
//     //     SDevice,
//     //     UDevice,
//     //     m,
//     //     VDevice,
//     //     n,
//     //     workDevice,
//     //     workSize,
//     //     info,
//     //     params,
//     //     batchSize);
//     // // if (CUSOLVER_STATUS_SUCCESS == statusBuffer)
//     // // {
//     // reconstructPhaseShiftMonocularCudaKernelStage2<<<block, thread>>>(decodeImageDevice, VDevice, n, reconstutionPoint);
//     // // cudaThreadSynchronize();
//     // //     }
//     // // }

//     // float *pointsHost = new float[length * 3];
//     // cudaMemcpy(pointsHost, reconstutionPoint, 3 * memSizeFloat, cudaMemcpyDeviceToHost);
//     // for (int i = 0; i < length; i++)
//     // {
//     //     if (!isnan(pointsHost[3 * i]))
//     //     {
//     //         int h = i / imageSize.width;
//     //         int w = i - h * imageSize.width;
//     //         Point3f p(pointsHost[3 * i], pointsHost[3 * i + 1], pointsHost[3 * i + 2]);
//     //         points.push_back(p);
//     //         float color = (float)image.at<uchar>(h, w);
//     //         Point3f colorPoint = {color, color, color};
//     //         colors.push_back(colorPoint);
//     //     }
//     // }
//     // cudaFree(ADevice);
//     // cudaFree(SDevice);
//     // cudaFree(UDevice);
//     // cudaFree(VDevice);
//     // cudaFree(workDevice);
//     // std::cout << "cuda finish" << std::endl;
// }
// }